#include "Swarm.h"
#include <QtCharts/QLineSeries>
#include<qdebug.h>

Swarm::Swarm()
{
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Swarm::_receiveMessage);
}

Swarm::~Swarm()
{
}

void Swarm::setTest1(QString test)
{
_test1=test;
qDebug()<<test;
emit valueChanged();
}

void Swarm::setTest2(QString test)
{
_test2=test;
qDebug()<<test;
emit valueChanged2();
}
void Swarm::sendStartCommand()
{
    start_swarm=true;
    if (!_vehicle)
    {
        qWarning() << "Internal error";
        return;
    }
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    _vehiclelist=manager->vehicles();

    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
    {

        for(int i=0;i<_vehiclelist->objectList()->count();i++)
        {
            QObject* obj =  _vehiclelist->get(i);
            Vehicle* vehicle0=static_cast<Vehicle*> (obj);
            if (vehicle0->id()==1)
            {
                WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
                if (!weakLink.expired()) {
                    SharedLinkInterfacePtr sharedLink = weakLink.lock();

                    if (!sharedLink) {
                        qCDebug(VehicleLog) << "_handlePing: primary link gone!";
                        return;
                    }
                    //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
                    auto priority_link =sharedLink;
                    mavlink_message_t msg;

                    mavlink_msg_gps_raw_int_pack_chan( 55,
                                                       55,
                                                      priority_link->mavlinkChannel(),
                                                      &msg,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0,
                                                      0);
                        vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                    }
                }
            }
        }



    }



void
Swarm::_receiveMessage(LinkInterface*, mavlink_message_t message)
{
if(message.msgid==MAVLINK_MSG_ID_GPS_RAW_INT)
{
    //qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
    mavlink_gps_raw_int_t gps_raw;
    mavlink_msg_gps_raw_int_decode(&message,&gps_raw);
   // qDebug()<<gps_raw.alt;
    if(message.sysid==1)
    {
    if (!_vehicle)
    {
                qWarning() << "Internal error";
                return;
    }
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    _vehiclelist=manager->vehicles();

    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
    {

                for(int i=0;i<_vehiclelist->objectList()->count();i++)
                {
                    QObject* obj =  _vehiclelist->get(i);
                    Vehicle* vehicle0=static_cast<Vehicle*> (obj);
                        WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
                        if (!weakLink.expired()) {
                        SharedLinkInterfacePtr sharedLink = weakLink.lock();

                        if (!sharedLink) {
                            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
                            return;
                        }
                        //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
                        auto priority_link =sharedLink;
                        mavlink_message_t msg;
//                        uint16_t yaw=1;
//                        if(start_swarm)
//                        {
//                            yaw=55;
//                        }
                        mavlink_msg_gps_raw_int_pack_chan( 1,
                                                          1,
                                                          priority_link->mavlinkChannel(),
                                                          &msg,
                                                          gps_raw.time_usec,
                                                          gps_raw.fix_type,
                                                          gps_raw.lat,
                                                          gps_raw.lon,
                                                          gps_raw.alt,
                                                          gps_raw.eph,
                                                          gps_raw.epv,
                                                          gps_raw.vel,
                                                          0,
                                                          0,
                                                          0,
                                                          0,
                                                          0,
                                                          0,
                                                          gps_raw.hdg_acc,
                                                          gps_raw.yaw);
                        vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                        }
                    }

    }
    }
}
}
